/*
 * Project: Hell Watch
 * Author: pajoke
 * Copyright: (C) 2014 Hell Prototypes
 * License: GNU GPL v3 (see License.txt)
 * Web: http://www.hellprototypes.com
 */

#include <avr/io.h>
#include <string.h>
#include <stdio.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include "common.h"
#include "devices/accelerate.h"
#include "drivers/i2c.h"
#include "millis/millis.h"
#include "watchconfig.h"

static bool accel_cmd(byte reg, byte cmd);
static bool accel_read(byte start_reg, byte *buffer, byte len);

void accel_init(void)
{
	accel_cmd(0x6c, 0x87);//20Hz frequency  of  wake-ups, puts the XYZ axis gyroscope into standby mode
	accel_wake();
	accel_cmd(0x37, 0xD0);//INT1: OD, Active: Low Level, No Latch INT, RD Clear INT

	//INT pin inited by pcf8563
}

char accel_get_temp(void)
{
	byte accd[2];
	int16_t temp;

    accel_read(0x41, accd, 2);

	temp = (accd[0]<<8) | accd[1];
	temp = (temp + 12420)/340;

    return (char)temp;
}

int16_t accel_get_y(void)
{
	byte accd[2];
	int16_t y_ch;

	if(watchConfig.acceleration) {
		accel_read(0x3d, accd, 2);
		y_ch = (accd[0]<<8) | accd[1];
		while((y_ch > 500) || (y_ch < -500)) {
			y_ch /= 2;
		}
	} else {
		y_ch = 0;
	}
	return y_ch;
}

int16_t accel_get_x(void)
{
	byte accd[2];
	int16_t x_ch;

	if(watchConfig.acceleration) {
		accel_read(0x3b, accd, 2);
		x_ch = (accd[0]<<8) | accd[1];
		while((x_ch > 500) || (x_ch < -500)) {
			x_ch /= 2;
		}
	} else {
		x_ch = 0;
	}

	return x_ch;
}
	
static bool accel_cmd(byte reg, byte cmd)
{
	return i2c_dev_write(0xD0, reg, cmd);
}

static bool accel_read(byte start_reg, byte *buffer, byte len)
{
	return i2c_dev_read(0xD0, start_reg, buffer, len);
}

void accel_sleep(void)
{
	if(watchConfig.acceleration) {
		//printf("accel_sleep\r\n");//FIXME
		byte threshold = watchConfig.sensitivity;
		threshold = threshold * 20 + 5;
		accel_cmd(0x38, 0x40);//Enable motion interrupt
		accel_cmd(0x1F, threshold);//Set detection threshold
		accel_cmd(0x20, threshold>>1);//Set duration counter threshold - 1ms
        //_delay_ms(5);
		accel_cmd(0x1c, 0x07);//Hold DHPF Force hardware to "lock" current accel sample.
		accel_cmd(0x6c, 0xB7);//only use ZA
	}else {
		accel_cmd(0x6b, 0x40);//enter sleep mode
	}
}

void accel_wake(void)
{
	if(watchConfig.acceleration) {
		//printf("accel_wake\r\n");//FIXME
		//accel_cmd(0x6b, 0x20);//exit deep suspend mode, enter cycle mode
		accel_cmd(0x6b, 0x00);
		accel_cmd(0x38, 0x00);//Disable motion interrupt
		accel_cmd(0x1c, 0x00);//Unlock DHPF
		byte int_status;
		accel_read(0x3A, &int_status, 1);
		accel_cmd(0x6c, 0x87);
		//printf("S=%02x\r\n",int_status);//FIXME
	}
}
